﻿#include "cmd_curve.h"
#include "ais/ais_pointcloud.h"
#include "algo/algo_approximate.h"
#include "algo/algo_interpolate.h"
#include <Dense>

CAGD::CmdStatus CAGD::CmdLagrange::Perform()
{
    // 计算插值点
    std::vector<BPnt> points, cps;
    for (auto ap : m_points)
        points.push_back(ap->GetPoint()->Point());

    int n = points.size();
    for (int i = 0; i < 100; i++)
    {
        double t = i * 1.0 / 99 * (n - 1);

        BPnt v(0, 0, 0);
        for (int j = 0; j < n; j++)
        {
            double p = 1, q = 1;
            for (int k = 0; k < n; k++)
            {
                if (j != k)
                {
                    p *= t - k;
                    q *= j - k;
                }
            }

            v = v + points[j] * p / q;
        }

        cps.push_back(v);
    }

    // 创建点云
    Handle(AIS_PointCloud) cloud(new AIS_PointCloud(cps));
    m_context.CreateObject(cloud);

    return CmdCurvePainter::Perform();
}

CAGD::CmdStatus CAGD::CmdBezierDyn::Perform()
{
    double a = 0.01, b = 1.04;

    auto Bin = [](int i, int n, double t) -> double {
        double Y = pow(1 - t, n - i) * pow(t, i);
        double c1 = 1, c2 = 1, c3 = 1;
        for (int j = 1; j <= n; j++)
            c1 *= j;
        for (int j = 1; j <= i; j++)
            c2 *= j;
        for (int j = 1; j <= n - i; j++)
            c3 *= j;

        return Y * c1 / c2 / c3;
    };

    std::vector<BPnt> points;
    for (auto ap : m_points)
        points.push_back(ap->GetPoint()->Point());
    int n = points.size() - 1;
    Eigen::MatrixXd CB(n + 1, n + 1);
    Eigen::MatrixXd MX(n + 1, n + 1);

    CB.setZero();
    MX.setZero();

    // 填充 CB
    for (int k = 0; k < n + 1; k++)
    {
        for (int l = 0; l < n + 1; l++)
        {
            double c = 0;
            for (int i = 0; i <= l; i++)
            {
                for (int j = 0; j <= n - l; j++)
                {
                    if (i + j == k)
                        c += Bin(i, l, b) * Bin(j, n - l, a);
                }
            }

            CB(k, l) = c;
        }
    }

    // 填充 MX
    for (int i = 0; i < n + 1; i++)
    {
        MX(0, i) = points[i].X();
        MX(1, i) = points[i].Y();
        MX(2, i) = points[i].Z();
    }
    for (int i = 3; i < n + 1; i++)
        MX(i, i - 3) = 1;

    // 计算 MB
    Eigen::MatrixXd MB = MX * CB * MX.inverse();

    // 初始点
    Eigen::MatrixXd X0(n + 1, 1);
    X0.setZero();

    // 初始参数
    double t0 = 0;
    BPnt p0 = points[0];

    // 重构点云
    std::vector<BPnt> cps = {p0};

    // 构建初值
    X0(0, 0) = p0.X();
    X0(1, 0) = p0.Y();
    X0(2, 0) = p0.Z();
    X0(3, 0) = 1;

    // 迭代计算
    for (int i = 0; i < 50; i++)
    {
        X0 = MB * X0;

        p0.SetX(X0(0, 0));
        p0.SetY(X0(1, 0));
        p0.SetZ(X0(2, 0));
        cps.push_back(p0);
    }

    // 创建点云
    Handle(AIS_PointCloud) cloud(new AIS_PointCloud(cps));
    m_context.CreateObject(cloud);

    return CmdCurvePainter::Perform();
}

CAGD::CmdStatus CAGD::CmdBezier::Perform()
{
    // 创建曲线
    std::vector<BPnt> points;
    for (auto ap : m_points)
        points.push_back(ap->GetPoint()->Point());
    m_curve = Handle(GeomCurve)(new GeomBezierCurve(points));
    return CmdCurveBuilder::Perform();
}

CAGD::CmdStatus CAGD::CmdBezierFit::Perform()
{
    // 指定次数
    std::vector<BPnt> points;
    for (auto ap : m_points)
        points.push_back(ap->GetPoint()->Point());
    m_curve = Approximate::BezierCurve(5, points);
    return CmdCurveBuilder::Perform();
}

CAGD::CmdStatus CAGD::CmdBSpline::Perform()
{
    // 创建曲线
    std::vector<BPnt> points;
    for (auto ap : m_points)
        points.push_back(ap->GetPoint()->Point());
    int size = points.size();
    m_curve = Handle(GeomCurve)(new GeomBSplineCurve(size > 3 ? 3 : (size - 1), points));
    return CmdCurveBuilder::Perform();
}

CAGD::CmdStatus CAGD::CmdBSplineFit::Perform()
{
    // 创建曲线
    std::vector<BPnt> points;
    for (auto ap : m_points)
        points.push_back(ap->GetPoint()->Point());
    m_curve = Approximate::BSplineCurve(3, 8, points);
    return CmdCurveBuilder::Perform();
}

CAGD::CmdStatus CAGD::CmdBSplineIntp::Perform()
{
    // 创建曲线
    std::vector<BPnt> points;
    for (auto ap : m_points)
        points.push_back(ap->GetPoint()->Point());
    m_curve = Interpolate::BSplineCurve3(points);
    return CmdCurveBuilder::Perform();
}

CAGD::CmdStatus CAGD::CmdBSplineLane::Perform()
{
    // 指定次数
    int d = 3;
    std::vector<BPnt> points;
    for (auto ap : m_points)
        points.push_back(ap->GetPoint()->Point());
    auto &cpts = points;

    // 迭代 3 次
    int t = 3;
    while (t > 0)
    {
        std::vector<BPnt> vertices;

        int n = cpts.size() - 1;
        for (int i = 0; i < 2 * n; i++)
        {
            if (i % 2 == 0)
                vertices.push_back(cpts[i / 2]);
            else
                vertices.push_back((cpts[(i - 1) / 2] + cpts[(i + 1) / 2]) / 2);
        }

        for (int k = 2; k < d; k++)
            for (int i = 0; i < vertices.size() - 1; i++)
                vertices[i] = (vertices[i] + vertices[i + 1]) / 2;

        cpts = vertices;
        t--;
    }

    // 创建点云
    Handle(AIS_PointCloud) cloud(new AIS_PointCloud(cpts));
    m_context.CreateObject(cloud);

    return CmdCurvePainter::Perform();
}

CAGD::CmdStatus CAGD::CmdBSplineDivide::Perform()
{
    // 次数
    int n = 3;

    // 双尺度系数
    std::vector<double> S;
    for (int i = 0; i < n + 2; i++)
    {
        double a = 1;
        for (int j = 0; j < i; j++)
            a = a * (n + 1 - j) / (j + 1);
        S.push_back(a / pow(2, n));
    }

    std::vector<BPnt> points;
    for (auto ap : m_points)
        points.push_back(ap->GetPoint()->Point());

    // 迭代 3 次
    int t = 3;
    while (t > 0)
    {
        std::vector<BPnt> record1;
        std::vector<BPnt> record2;
        std::vector<int> index1;
        std::vector<int> index2;
        Subdivide(0, points, record1, index1, S);
        Subdivide(1, points, record2, index2, S);

        int ind1 = 0, ind2 = 0;
        points.clear();
        while (ind1 < index1.size() && ind2 < index2.size())
        {
            if (index1[ind1] > index2[ind2])
            {
                points.push_back(record2[ind2]);
                ind2++;
            }
            else
            {
                points.push_back(record1[ind1]);
                ind1++;
            }
        }
        for (int i = ind1; i < index1.size(); i++)
            points.push_back(record1[ind1]);
        for (int i = ind2; i < index2.size(); i++)
            points.push_back(record2[ind2]);

        t--;
    }

    // 创建点云
    Handle(AIS_PointCloud) cloud(new AIS_PointCloud(points));
    m_context.CreateObject(cloud);

    return CmdCurvePainter::Perform();
}

void CAGD::CmdBSplineDivide::Subdivide(int even, std::vector<BPnt> &points, std::vector<BPnt> &record,
                                       std::vector<int> &index, std::vector<double> &S)
{
    int c = S.size() / 2 + S.size() % 2 - 2 + even;
    int ind = 1 - even;

    for (int i = 0; i < points.size(); i++)
    {
        BPnt P = points[i] * S[c];

        int l = 1, r = 1;
        bool valid = true;

        // 先计算左侧点的和
        while (c - l * 2 >= 0)
        {
            // 如果对应控制点不存在，则此点无效
            if (i - l < 0)
            {
                valid = false;
                break;
            }

            P = P + points[i - l] * S[c - l * 2];
            l++;
        }

        // 再计算右侧点的和
        while (c + r * 2 < S.size())
        {
            // 如果对应控制点不存在，则此点无效
            if (i + r >= points.size())
            {
                valid = false;
                break;
            }

            P = P + points[i + r] * S[c + r * 2];
            r++;
        }

        // 只记录有效点
        if (valid)
        {
            record.push_back(P);
            index.push_back(2 * i + 1 - even);
            ind += 2;
        }
    }
}
